#include <XnOpenNI.h>
#include <XnLog.h>
#include <XnCppWrapper.h>
#include <XnFPSCalculator.h>
#include <stdio.h>
#include "cv.h"
#include "highgui.h"
#include <iostream>
#include <fstream>
#include <math.h>
#include <boost/filesystem.hpp>
#include <boost/regex.hpp>
#include <string.h>
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
//TODO
//Hantera flera typer utav objekt i samma scen
//Mask för polygoner
//Minnesläckor?

#define CHECK_RC(rc, what)											\
  if (rc != XN_STATUS_OK)											\
{																\
  printf("%s failed: %s\n", what, xnGetStatusString(rc));		\
  return rc;													\
}

#define SAMPLE_XML_PATH "SamplesConfig.xml"

std::string read_path;
std::string save_path;
std::string item_type;
std::vector<string> all_files;
std::vector<string>::iterator it;
using namespace std;

struct selection {
  int mode;
  void * data;
  selection * next;
};

struct selection_circle {
  int x;
  int y;
  int radius;
};

struct selection_box {
  int top_y;
  int bot_y;
  int left_x;
  int right_x;
};

struct selection_polygon {
  int x;
  int y;
  selection_polygon * next;
};

int frame_number = 2;
bool making_box = false;
int box_x_start = -1;
int box_y_start = -1;

bool making_polygon = false;
selection_polygon * current_polygon_start = 0;
selection_polygon * current_polygon = current_polygon_start;


int mode = 2;//1 = point, 2 = box, 3 = poly
float circle_radius = 20;

//Display studd
bool display_information = true;
bool multi_patch = true;
int last_mouse_x = -1;
int last_mouse_y = -1;
IplImage * work_image = 0;
IplImage * display_image = 0;
IplImage * drawing_image = 0;
selection * current_selection_start;
selection * current_selection = current_selection_start;
selection * temp_sel;

void redraw();
void mouseHandler(int event, int x, int y, int flags, void *param);
void next_frame();
void previous_frame();
void set_mode(int modenr);
void save_selection(selection * to_save);
void redraw_simple();
int multiplier_size = 1;

bool stringCompare(string s1, string s2){
  std::vector<std::string> strs1,strs2;
  boost::split(strs1, s1, boost::is_any_of("_"));
  boost::split(strs2, s2, boost::is_any_of("_"));
  int i1 = boost::lexical_cast<int>(strs1[1]);
  int i2 = boost::lexical_cast<int>(strs2[1]);
  if (i1 > i2)
    return false;
  else
    return true;
}
std::vector<std::string> getFileList(std::string target_path){

  std::vector< std::string > all_matching_files;

  boost::filesystem::directory_iterator end_itr; // Default ctor yields past-the-end
  for( boost::filesystem::directory_iterator i( target_path ); i != end_itr; ++i )
  {
    // Skip if not a file
    if( !boost::filesystem::is_regular_file( i->status() ) ) continue;
     if ( i->leaf().find("rgb") == string::npos) continue;
    // File matches, store it
    all_matching_files.push_back( target_path + "/" + i->leaf() );
  }
  std::sort(all_matching_files.begin(), all_matching_files.end(),stringCompare);
  return all_matching_files;
}
void myShowImage(char * window_name, IplImage * image_to_show)
{
  //cvShowImage("image", work_image);
  IplImage* tmp_image=cvCreateImage(cvSize(640 * multiplier_size,480 * multiplier_size),IPL_DEPTH_8U,3);
  for(int i = 0; i <  640*multiplier_size;i+=multiplier_size)
  {
    for(int j = 0; j < 480*multiplier_size;j+=multiplier_size)
    {
      int r = image_to_show->imageData[3*((j/multiplier_size)*640+(i/multiplier_size))+0];
      int g = image_to_show->imageData[3*((j/multiplier_size)*640+(i/multiplier_size))+1];
      int b = image_to_show->imageData[3*((j/multiplier_size)*640+(i/multiplier_size))+2];
      for(int mi = 0; mi < multiplier_size;mi++){
        for(int mj = 0; mj < multiplier_size;mj++){
          tmp_image->imageData[3*((j+mj)*(640*multiplier_size)+(i+mi))+0]=r;
          tmp_image->imageData[3*((j+mj)*(640*multiplier_size)+(i+mi))+1]=g;
          tmp_image->imageData[3*((j+mj)*(640*multiplier_size)+(i+mi))+2]=b;
        }
      }
    }
  }
  cvShowImage("image", tmp_image);
  cvReleaseImage(&tmp_image);
}

IplImage * scaleImage(IplImage * image_to_show)
{
  IplImage* tmp_image=cvCreateImage(cvSize(640 * multiplier_size,480 * multiplier_size),IPL_DEPTH_8U,3);
  for(int i = 0; i <  640*multiplier_size;i+=multiplier_size)
  {
    for(int j = 0; j < 480*multiplier_size;j+=multiplier_size)
    {
      int r = image_to_show->imageData[3*((j/multiplier_size)*640+(i/multiplier_size))+0];
      int g = image_to_show->imageData[3*((j/multiplier_size)*640+(i/multiplier_size))+1];
      int b = image_to_show->imageData[3*((j/multiplier_size)*640+(i/multiplier_size))+2];
      for(int mi = 0; mi < multiplier_size;mi++){
        for(int mj = 0; mj < multiplier_size;mj++){
          tmp_image->imageData[3*((j+mj)*(640*multiplier_size)+(i+mi))+0]=r;
          tmp_image->imageData[3*((j+mj)*(640*multiplier_size)+(i+mi))+1]=g;
          tmp_image->imageData[3*((j+mj)*(640*multiplier_size)+(i+mi))+2]=b;
        }
      }
    }
  }
  //cvShowImage("image", tmp_image);
  cvReleaseImage(&image_to_show);
  //image_to_show = tmp_image;
  return tmp_image;
}

void mouseHandler(int event, int x, int y, int flags, void *param) {
  last_mouse_x = x;
  last_mouse_y = y;
  switch (event) {
    case CV_EVENT_LBUTTONDOWN: {
                                switch (mode) {
                                   case 1:
                                     {

                                       current_selection->mode = 1;
                                       selection_circle * tmp_circle = new selection_circle;
                                       tmp_circle->x = x;
                                       tmp_circle->y = y;
                                       tmp_circle->radius = circle_radius;
                                       current_selection->data = tmp_circle;
                                       if (multi_patch) {
                                         current_selection->next = new selection;
                                         current_selection = current_selection->next;
                                         current_selection->next = 0;
                                         current_selection->mode = 0;
                                         current_selection->data = 0;
                                      } else {
                                         save_selection(current_selection_start);
                                         next_frame();
                                       }
                                     }
                                     break;
                                   case 2:
                                     {
                                       making_box = true;
                                       box_x_start = x;
                                       box_y_start = y;
                                     }
                                     break;
                                   case 3:
                                     if(making_polygon)
                                     {
                                       if(sqrt(((current_polygon_start->x)-x)*((current_polygon_start->x)-x)+((current_polygon_start->y)-y)*((current_polygon_start->y)-y)) < 8)
                                       {
                                         if(current_polygon_start->next != 0 && current_polygon_start->next->next != 0 )
                                         {
                                           printf("finish polygon\n");
                                           current_selection->mode = 3;
                                           current_selection->data = current_polygon_start;
                                           if (multi_patch) {
                                             current_selection->next = new selection;
                                             current_selection = current_selection->next;
                                             current_selection->next = 0;
                                             current_selection->mode = 0;
                                             current_selection->data = 0;
                                           } else {
                                             save_selection(current_selection_start);
                                             next_frame();
                                           }
                                           making_polygon = false;
                                           current_polygon = 0;
                                           current_polygon_start = 0;
                                         }
                                       }
                                       else{
                                         printf("new polygon part\n");
                                         current_polygon->next = new selection_polygon;
                                         current_polygon = current_polygon->next;
                                         current_polygon->x = x;
                                         current_polygon->y = y;
                                         current_polygon->next = 0;
                                       }
                                     }else
                                     {
                                       printf("new polygon\n");
                                       making_polygon = true;
                                       current_polygon_start = new selection_polygon;
                                       current_polygon = current_polygon_start;
                                       current_polygon->x = x;
                                       current_polygon->y = y;
                                       current_polygon->next = 0;
                                     }
                                     break;
                                 }
                               }
                               redraw();
                               break;
    case CV_EVENT_LBUTTONUP: {
                               if(mode == 2 && making_box){
                                 current_selection->mode = 2;
                                 selection_box * tmp_box = new selection_box;
                                 if(y < box_y_start){
                                   tmp_box->top_y = y;
                                   tmp_box->bot_y = box_y_start;
                                 }else{
                                   tmp_box->top_y = box_y_start;
                                   tmp_box->bot_y = y;
                                 }
                                 if(x < box_x_start){
                                   tmp_box->left_x = x;
                                   tmp_box->right_x = box_x_start;
                                 }else{
                                   tmp_box->left_x = box_x_start;
                                   tmp_box->right_x = x;
                                 }
                                 current_selection->data = tmp_box;
                                 if (multi_patch) {
                                   current_selection->next = new selection;
                                   current_selection = current_selection->next;
                                   current_selection->next = 0;
                                   current_selection->mode = 0;
                                   current_selection->data = 0;
                                 } else {
                                   save_selection(current_selection_start);
                                   next_frame();
                                 }
                               }
                               making_box = false;
                               box_x_start = -1;
                               box_y_start = -1;
                             }
                             redraw();
                             break;
    case CV_EVENT_RBUTTONDOWN: {
                                 box_x_start = x; 
                                 box_y_start = y;  
                               } break;
    case CV_EVENT_MOUSEMOVE: {}redraw_simple();break;
  }

}

void redraw() {
  //printf("redraw\n");
  cvReleaseImage(&drawing_image);
  cvReleaseImage(&display_image);
  drawing_image = cvCloneImage(work_image);
  selection * current = current_selection_start;
  while (current != 0) {
    switch(current->mode)
    {
      case 1:
        {
          selection_circle * tmp = (selection_circle *)(current->data);
          cvCircle(drawing_image, cvPoint(tmp->x, tmp->y),tmp->radius, cvScalar(0, 0, 255, 0), 1);
        }break;
      case 2:{
               selection_box * tmp = (selection_box *)(current->data);
               int ty = tmp->top_y;
               int tx = tmp->left_x;
               int by = tmp->bot_y;
               int bx = tmp->right_x;
               cvRectangle(drawing_image, cvPoint(tx, ty), cvPoint(bx, by),cvScalar(0, 0, 255, 0), 1, 8, 0);
             }break;
      case 3:{
               selection_polygon * current_polygon_tmp = (selection_polygon *)(current->data);
               selection_polygon * current_polygon_tmp_start = current_polygon_tmp;
               while(current_polygon_tmp!=0)
               {
                 if(current_polygon_tmp->next != 0)
                 {
                   cvLine(drawing_image, cvPoint(current_polygon_tmp->x, current_polygon_tmp->y),
                       cvPoint(current_polygon_tmp->next->x, current_polygon_tmp->next->y), cvScalar(0, 0, 255), 1);
                 }
                 else{

                   cvLine(drawing_image, cvPoint(current_polygon_tmp->x, current_polygon_tmp->y),
                       cvPoint(current_polygon_tmp_start->x, current_polygon_tmp_start->y), cvScalar(0, 0, 255), 1);
                 }
                 current_polygon_tmp = current_polygon_tmp->next;
               }
             }break;
    }
    current = current->next;
  }
  display_image = cvCloneImage(drawing_image);

  switch (mode) {
    case 1:
      cvCircle(display_image, cvPoint(last_mouse_x, last_mouse_y),(int) circle_radius, cvScalar(0, 255, 0, 0), 1);
      break;
    case 2:
      if (making_box) {
        cvRectangle(display_image, cvPoint(box_x_start, box_y_start),cvPoint(last_mouse_x, last_mouse_y),cvScalar(0, 255, 0, 0), 1, 8, 0);
      }
      break;
    case 3:
      if(making_polygon)
      {
        selection_polygon * current_polygon_tmp = current_polygon_start;
        while(current_polygon_tmp!=0)
        {
          //printf(".");
          if(current_polygon_tmp->next != 0)
          {
            cvLine(display_image, cvPoint(current_polygon_tmp->x, current_polygon_tmp->y),cvPoint(current_polygon_tmp->next->x, current_polygon_tmp->next->y), cvScalar(0, 0, 255), 1);
          }
          current_polygon_tmp = current_polygon_tmp->next;
        }
        //printf("\n");

        cvLine(display_image, cvPoint(current_polygon->x, current_polygon->y),cvPoint(last_mouse_x, last_mouse_y), cvScalar(0, 255, 0), 1);

      }
      break;
  }
  /*
     Overlay showing information about current mode and such
   */
  if (display_information) {
    CvFont font;
    cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.4, 0.4, 0, 1, CV_AA);
    char buf[512];
    char * temp1;
    if (mode == 1) {
      temp1 = (char *) "circle  ";
    } else if (mode == 2) {
      temp1 = (char *) "box    ";
    } else if (mode == 3) {
      temp1 = (char *) "polygon";
    } else {
      temp1 = (char *) "Unknown";
    }
    char * temp2;
    if (multi_patch) {
      temp2 = (char *) "on";
    } else {
      temp2 = (char *) "off";
    }
    sprintf(buf,
        "Image:frame%d_depth.bmp  Mode:%s Select multiple patches:%s  ",
        frame_number, temp1, temp2);
    cvPutText(display_image, buf, cvPoint(1, 15), &font, cvScalar(0, 0,
          255, 0));
    cvShowImage("image", display_image);//cvShowImage("image", display_image);
  } else {
    cvShowImage("image", display_image);//cvShowImage("image", display_image);
  }

}

void redraw_simple() {
  //printf("redraw\n");
  cvReleaseImage(&display_image);
  display_image = cvCloneImage(drawing_image);

  switch (mode) {
    case 1:
      cvCircle(display_image, cvPoint(last_mouse_x, last_mouse_y),
          (int) circle_radius, cvScalar(0, 255, 0, 0), 1);
      break;
    case 2:
      if (making_box) {
        cvRectangle(display_image, cvPoint(box_x_start, box_y_start),
            cvPoint(last_mouse_x, last_mouse_y),
            cvScalar(0, 255, 0, 0), 1, 8, 0);
      }
      break;
    case 3:
      if(making_polygon)
      {
        selection_polygon * current_polygon_tmp = current_polygon_start;
        while(current_polygon_tmp!=0)
        {
          //printf(".");
          if(current_polygon_tmp->next != 0)
          {
            cvLine(display_image, cvPoint(current_polygon_tmp->x, current_polygon_tmp->y),
                cvPoint(current_polygon_tmp->next->x, current_polygon_tmp->next->y), cvScalar(0, 0, 255), 1);
          }
          current_polygon_tmp = current_polygon_tmp->next;
        }
        //printf("\n");

        cvLine(display_image, cvPoint(current_polygon->x, current_polygon->y),
            cvPoint(last_mouse_x, last_mouse_y), cvScalar(0, 255, 0), 1);

      }
      break;
  }
  /*
     Overlay showing information about current mode and such
   */
  if (display_information) {
    CvFont font;
    cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 0.4, 0.4, 0, 1, CV_AA);
    char buf[512];
    char * temp1;
    if (mode == 1) {
      temp1 = (char *) "circle  ";
    } else if (mode == 2) {
      temp1 = (char *) "box    ";
    } else if (mode == 3) {
      temp1 = (char *) "polygon";
    } else {
      temp1 = (char *) "Unknown";
    }
    char * temp2;
    if (multi_patch) {
      temp2 = (char *) "on";
    } else {
      temp2 = (char *) "off";
    }
    sprintf(buf,
        "Image:frame%d_depth.bmp  Mode:%s Select multiple patches:%s  ",
        frame_number, temp1, temp2);
    cvPutText(display_image, buf, cvPoint(1, 15), &font, cvScalar(0, 0,
          255, 0));
    cvShowImage("image", display_image);//cvShowImage("image", display_image);
  } else {
    cvShowImage("image", display_image);//cvShowImage("image", display_image);
  }

}
void next_frame() {
  printf("Next frame\n");
  it++;
  if(it == all_files.end()){
    printf("All files are finished. \n");
  }
  making_box = false;
  box_x_start = -1;
  box_y_start = -1;
  making_polygon = false;
  current_polygon_start = 0;
  current_polygon = current_polygon_start;
  current_selection_start = new selection;
  current_selection_start->next = 0;
  current_selection_start->mode = 0;
  current_selection_start->data = 0;
  current_selection=current_selection_start;

  char buf[256];
  sprintf(buf, "%s",it->c_str());
  cvReleaseImage(&work_image);
  int width_resolution = 640;
  int height_resolution = 480;
  work_image = cvLoadImage(buf, CV_LOAD_IMAGE_COLOR);
  work_image = scaleImage(work_image);
  //work_image = cvLoadImage(buf, CV_LOAD_IMAGE_COLOR);
  if (work_image == 0) {
    fprintf(stderr, "Cannot load file %s!\n", buf);
    fprintf(stderr, "Shuting down program\n");
    cvDestroyWindow("image");
    //cvDestroyWindow("image2");
    cvReleaseImage(&work_image);
    cvReleaseImage(&drawing_image);
    cvReleaseImage(&display_image);
    exit(0);
  }
  /*for(int i = 0; i <  width_resolution;i++)
    {
    for(int j = 0; j < height_resolution;j++)
    {
    int distance_from_cam = 0;
    int part1 = 0xFF&work_image->imageData[3*(j*width_resolution+i)+0];
    int part2 = 0xFF&work_image->imageData[3*(j*width_resolution+i)+1];
    distance_from_cam = (part1<<8)+part2;
    char tmp = (char)(distance_from_cam/8);
    work_image->imageData[3*(j*width_resolution+i)+0]=tmp;
    work_image->imageData[3*(j*width_resolution+i)+1]=tmp;
    work_image->imageData[3*(j*width_resolution+i)+2]=tmp;
    }
    }*/
  //int N = 7;
  //printf("....tes...\n");
  //IplImage * frame2 = cvCreateImage(cvSize(work_image->width, work_image->height),IPL_DEPTH_8U, 1);
  //cvCvtColor(work_image,  frame2, CV_BGR2GRAY) ;
  //cvCanny(  frame2,   frame2, 0.3, 0.8);
  //cvCanny( frame2, frame2, 1*N*N, 40*N*N, N );
  //cvCvtColor(frame2,work_image, CV_GRAY2BGR) ;
  //cvReleaseImage(&frame2);
  /*
     IplImage * work_image2 = cvLoadImage(buf, CV_LOAD_IMAGE_COLOR);
     for(int i = 0; i <  width_resolution;i++)
     {
     for(int j = 0; j < height_resolution;j++)
     {
     int distance_from_cam = 0;
     int part1 = 0xFF&work_image2->imageData[3*(j*width_resolution+i)+0];
     int part2 = 0xFF&work_image2->imageData[3*(j*width_resolution+i)+1];
     distance_from_cam = (part1<<8)+part2;
     char tmp = (char)(distance_from_cam/8);
     work_image->imageData[3*(j*width_resolution+i)+0]+=tmp;
     work_image->imageData[3*(j*width_resolution+i)+1]+=tmp;
     work_image->imageData[3*(j*width_resolution+i)+2]+=tmp;
     }
     }
     cvReleaseImage(&work_image2);
   */
  redraw();
}
void previous_frame() {
  printf("prev\n");
  if(it != all_files.begin()){
    it--;
    making_box = false;
    box_x_start = -1;
    box_y_start = -1;
    making_polygon = false;
    current_polygon_start = 0;
    current_polygon = current_polygon_start;
    current_selection_start = new selection;
    current_selection_start->next = 0;
    current_selection_start->mode = 0;
    current_selection_start->data = 0;
    current_selection=current_selection_start;
  
    char buf[256];
    sprintf(buf, "%s",it->c_str());
    cvReleaseImage(&work_image);
    work_image = cvLoadImage(buf, CV_LOAD_IMAGE_COLOR);
    work_image = scaleImage(work_image);
    if (work_image == 0) {
      fprintf(stderr, "Cannot load file %s!\n", buf);
      fprintf(stderr, "Shuting down program\n");
      cvDestroyWindow("image");
      //cvDestroyWindow("image2");
      cvReleaseImage(&work_image);
      cvReleaseImage(&drawing_image);
      cvReleaseImage(&display_image);
      exit(0);
    }
    redraw();
  }
  else{
    printf("Already at the first frame \n");
  }
}
void set_mode(int modenr) {
  printf("set mode nr: %i\n", modenr);
  making_box = false;
  box_x_start = -1;
  box_y_start = -1;
  making_polygon = false;
  current_polygon_start = 0;
  current_polygon = current_polygon_start;
  switch (modenr) {
    case 0:
      printf("No mode:0\n");
      break;
    case 1:
      mode = 1;
      redraw();
      break;
    case 2:
      mode = 2;
      redraw();
      break;
    case 3:
      mode = 3;
      redraw();
      break;
  }
}

void save_selection(selection * to_save) {
  printf("Save selection\n");
  IplImage* classimage=cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,3);
  selection * current = to_save;
  int nr_polys = 0;
  while (current != 0) {
    if(current->mode == 3){nr_polys++;}
    current = current->next;
  }
  printf("nr_polys = %i\n",nr_polys);
  //CvPoint ** polys = new CvPoint*[nr_polys];
  CvPoint ** curveArr 	= new CvPoint*[nr_polys];
  int * nCurvePts		= new int[nr_polys];
  int nCurves	 	= 0;
  current = to_save;
  // if there's no annotation don't save just skip it
  if (current->data == 0){
    printf("Not saving mask for this frame \n");
    return;
    }

  while (current != 0) {
    switch(current->mode)
    {
      case 1:
        {
          selection_circle * tmp = (selection_circle *)(current->data);
          cvCircle(classimage, cvPoint(tmp->x/multiplier_size, tmp->y/multiplier_size),tmp->radius/multiplier_size, cvScalar(255, 255, 255, 0), CV_FILLED);
        }break;
      case 2:{
               selection_box * tmp = (selection_box *)(current->data);
               int ty = tmp->top_y;
               int tx = tmp->left_x;
               int by = tmp->bot_y;
               int bx = tmp->right_x;
               cvRectangle(classimage, cvPoint(tx/multiplier_size, ty/multiplier_size), cvPoint(bx/multiplier_size, by/multiplier_size),cvScalar(255, 255, 255, 0), CV_FILLED, 8, 0);
             }break;
      case 3:{
               selection_polygon * current_polygon_tmp = (selection_polygon *)(current->data);
               selection_polygon * current_polygon_tmp_start = current_polygon_tmp;
               int nr_points = 0;
               while(current_polygon_tmp!=0)
               {
                 /*if(current_polygon_tmp->next != 0)
                   {
                   cvLine(classimage, cvPoint(current_polygon_tmp->x, current_polygon_tmp->y),cvPoint(current_polygon_tmp->next->x, current_polygon_tmp->next->y), cvScalar(0, 0, 255), 3);
                   }
                   else{

                   cvLine(classimage, cvPoint(current_polygon_tmp->x, current_polygon_tmp->y),cvPoint(current_polygon_tmp_start->x, current_polygon_tmp_start->y), cvScalar(0, 0, 255), 3);
                   }*/
                 current_polygon_tmp = current_polygon_tmp->next;
                 nr_points++;
               }
               if (nr_points > 0){
                 CvPoint * pts = new CvPoint[nr_points];
                 nr_points = 0;
                 current_polygon_tmp = (selection_polygon *)(current->data);
                 current_polygon_tmp_start = current_polygon_tmp;
                 while(current_polygon_tmp!=0)
                 {
                   pts[nr_points].x = current_polygon_tmp->x/multiplier_size;
                   pts[nr_points].y = current_polygon_tmp->y/multiplier_size;
                   current_polygon_tmp = current_polygon_tmp->next;
                   nr_points++;
                 }
                 //CvPoint* curveArr[1]={pts};
                 //int      nCurvePts[1]={nr_points};
                 //CvPoint ** curveArr 	= new CvPoint*[1];	
                 curveArr[nCurves]	=pts;
                 nCurvePts[nCurves]	=nr_points;
                 nCurves++;
                 //cvFillPoly(classimage,curveArr,nCurvePts,nCurves,cvScalar(255,255,255));
                 //delete pts;  
               }

             }break;
    }
    current = current->next;
  }
  cvFillPoly(classimage,curveArr,nCurvePts,nCurves,cvScalar(255,255,255));
  char buf[256];
  std::string imagePath = save_path +"/" + item_type;
  std::vector<std::string> strs;
  boost::split(strs, *it, boost::is_any_of("/"));
  sprintf(buf,"%s/%s_mask.bmp",imagePath.c_str(), strs.at(strs.size()-1).c_str());

  printf("Saving mask to %s \n",buf);
  cvSaveImage(buf, classimage);
  cvReleaseImage(&classimage);
  printf("saved.");
}

int main(int argc, char** argv) {
  int i = 0;
  printf("%d arguments given \n", argc);
  for(i = 0; i < argc;i++)
  {
    printf(".%s\n",argv[i]);
    if(!strcmp(argv[i], "--item"))
    {
      printf("Annotating items of type: %s\n",argv[++i]);
      item_type = argv[i];
    }
    if(!strcmp(argv[i], "--read_path"))
    {
      printf("read_path: %s\n",argv[++i]);
      if(argv[i] == "."){
        read_path = boost::filesystem::current_path().string();
      }

      read_path = argv[i];
    }

  if(!strcmp(argv[i], "--save_path"))
    {
      printf("save_path: %s\n",argv[++i]);
      if(argv[i] == "."){
        save_path = boost::filesystem::current_path().string();
      }else{
        save_path = argv[i];
      }
    }
		
  }
     if (save_path == "" && read_path == ""){
      save_path = boost::filesystem::current_path().string();
      read_path = boost::filesystem::current_path().string();
      printf("No path is given, assuming current directory (%s) for both \n", save_path.c_str());
    }
	
  all_files = getFileList(read_path);
  printf("Containts %d files", all_files.size());
  it = all_files.begin();
  boost::filesystem::create_directories(save_path + "/" + item_type); 
  current_selection_start = new selection;
  current_selection_start->next = 0;
  current_selection_start->mode = 0;
  current_selection_start->data = 0;
  current_selection=current_selection_start;
  work_image = cvLoadImage(it->c_str(), CV_LOAD_IMAGE_COLOR);
  if (work_image == 0) {
    fprintf(stderr, "Cannot load file %s!\n", it->c_str());
    return 1;
  }
  work_image = scaleImage(work_image);

  cvNamedWindow("image", CV_WINDOW_AUTOSIZE);
  cvShowImage("image", work_image);//cvShowImage("image", work_image);
  it++; 
  redraw();
  cvSetMouseCallback("image", mouseHandler, NULL );
  while (true) {
    char tmp = char(cvWaitKey(0));
    switch (tmp) {
      case 'w':
        save_selection(current_selection_start);
        next_frame();
        break;//Right arrow
      case 's':
        previous_frame();
        break;//Left arrow
      case '1':
        set_mode(1);
        break;//1
      case '2':
        set_mode(2);
        break;//2
      case '3':
        set_mode(3);
        break;//3
      case 'q':
        printf("Display info changed\n");
        display_information = !display_information;
        redraw();
        break;//q
          case 'm':
        printf("Multipatch changed\n");
        multi_patch = !multi_patch;
        redraw();
        break;//w
      case 'e':
        printf("circle_radius increesed\n");
        circle_radius++;
        redraw();
        break;//e
      case 'r':
        printf("circle_radius decreased\n");
        if (circle_radius > 1) {
          circle_radius--;
        }
        redraw();
        break;//r
     case 'h':
      printf("\nHELP \n"
      "press: \n"
      "1 : to annotate in circle mode \n"
      "2 : to annotate in bounding box mode \n"
      "3 : to annotate in polygon mode \n"
      "m : to annotate in multi-patch mode \n"
      "w : to next frame \n"
      "s : to previous frame \n"
      "q : to toggle info in the window \n"
      "h : to display this help \n\n");
      break;
 }
  }
  cvDestroyWindow("image");
  //cvDestroyWindow("image2");
  cvReleaseImage(&work_image);
  cvReleaseImage(&drawing_image);
  cvReleaseImage(&display_image);
  return 0;
}
